#include "WPILib.h"
#include "Robot2489.h"

	
	/**
	 * This is the STATIC kickFunction that merely returns an INSTANCE of the
	 * kickFunction; we must do this in order to make the Task work
	 */
	/*int Robot2489::s_kickFunction()
	{
		return Robot2489::getInstance().kickFunction();
	}*/
	

	/**
	 * This is the actual definition of the kickFunction that contains the actual
	 * commands.
	 */

extern bool kicking;

	void Robot2489::kickFunction(void)
	{
		OpenLatch();
		Wait(.7);
		RetractKicker();
		Wait(.7);
		CloseLatch();
		Wait(0.01);
		kicking = false;
		OpenLatch();
		Wait(LATCHOPENTIME);
		RetractKicker();
		Wait(KICKERRETRACTTIME);
		CloseLatch();
		Wait(LATCHCLOSETIME);
		ChargeKicker();
		Wait(KICKERREADYINGTIME);
		
		//return 0;
	}

	
	/**
	 * ChargeKicker arms the kicker while the gate latch is still closed so that
	 * the kick will be more powerful when it's released, because the kicker
	 * will be trying to go with the bungie cords' force. This function is only
	 * called when we want to charge the kicker for a STRONG kick.
	 */
	void Robot2489::ChargeKicker(void)
	{
		// Arm the kicker.
		solKickIn->Set(0);
		solKickOut->Set(1);
	}

	/**
	 * RelaxKicker makes the kicker solenoids both turn off, so that when the 
	 * gate latch releases it the kick will be a MEDIUM powered kick. 
	 */
	void Robot2489::RelaxKicker(void) 
	{
		// Turn off both kickers. 
		solKickIn->Set(0);
		solKickOut->Set(0);
	}
	
	/**
	 * RetractKicker makes the kicker go back in after a kick, so we can kick 
	 * again. 
	 * 
	 * We also 
	 * use this function to anti-charge the kicker while the gate latch is still 
	 * closed so that the kick will be less powerful when it's released, because
	 * the kicker will be actively resisting the pull of the bungie cords. 
	 * We then use this function to ANTI-charge the kicker for a WEAK kick.
	 */
	void Robot2489::RetractKicker(void) 
	{
		solKickIn->Set(1);
		solKickOut->Set(0);
	}

	/**
	 * OpenLatch opens the gate latch so that the kicker kicks.
	 */
	void Robot2489::OpenLatch(void)
	{
		// Opens the latch.
		solLatch->Set(1);
	}
	
	
	/**
	 * CloseLatch closes the gate latch after the kicker retracts.
	 */
	void Robot2489::CloseLatch(void)
	{
		// Close the latch.
		solLatch->Set(0);
	}
	
	void Robot2489::RaiseTower()
	{
		solTower->Set(1);
	}
		
	void Robot2489::RetractTower()
	{
		solTower->Set(0);
	}
	